﻿using DevExpress.ClipboardSource.SpreadsheetML;
using DevExpress.Data.Filtering.Helpers;
using DevExpress.Security;
using DevExpress.Xpo.DB;
using DevExpress.XtraEditors.Camera;
using DevExpress.XtraExport.Helpers;
using DMSkin.Socket;
using DMSkin.Win32;
using HalconDotNet;
using MvCamCtrl.NET;
using MvCamCtrl.NET.CameraParams;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Net;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;
using static DevExpress.Utils.Svg.CommonSvgImages;
using static System.Windows.Forms.VisualStyles.VisualStyleElement;

namespace HTHMachineVision
{
    public partial class Form1 : Form
    {
        //图像变量
        HImage m_hImage = new HImage();
        //窗口句柄
        HWindow hWindow = null;

        //数据对象
        MyData myData = new MyData();

        CameraService cameraService = new CameraService();

        //TCP通信服务
        TCPService tcpService = new TCPService();

        bool _auto_function_thread = false;

        //当前匹配模板结果信息 记录A~
        double m_CurModel_Row = 0;
        double m_CurModel_Col = 0;
        double m_CurModel_Angle = 0;

        string saveFile = "template.hth";

        HRegion region_最终区域;

        int outTime = 100000;


        public Form1()
        {
            InitializeComponent();
            //赋值窗口句柄
            hWindow = hWindow_Final1.getHWindowControl().HalconWindow;
            //读取保存参数
            ReadParameterData();

            comboBox1_点选择.SelectedIndex = 0;
            comboBox1_过渡方式.SelectedIndex = 0;

            ReadRobotData();
        }

        /// <summary>
        /// 读取图像
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button_readImg_Click(object sender, EventArgs e)
        {
            //视觉一般使用 bmp tif 格式的，因为这2个可以保存原图
            //设置对话框的过滤器 （缩略图 png jpg gif）
            openFileDialog1.Filter = "图像文件|*.bmp;*.png;*.jpg;*.tif";
            //打开文件对话框
            if (openFileDialog1.ShowDialog() == DialogResult.OK)
            {
                //获得选中文件的路径
                string fileName = openFileDialog1.FileName;
                //读取图像
                m_hImage.ReadImage(fileName);
                //使用新控件显示图像 —>> 显示图像类型都用他
                hWindow_Final1.HobjectToHimage(m_hImage);
            }
        }

        #region 相机采集图像

        /// <summary>
        /// 打开设备
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bnOpen_Click(object sender, EventArgs e)
        {
            try
            {
                //有设备的情况下才能创建连接
                if (cameraService.m_ltDeviceList.Count == 0 || cbDeviceList.SelectedIndex == -1)
                {
                    cameraService.ShowErrorMsg("No device, please select", 0);
                    return;
                }

                // ch:获取选择的设备信息 -->> 下拉框当前选中的
                CCameraInfo device = cameraService.m_ltDeviceList[cbDeviceList.SelectedIndex];

                // ch:打开设备 
                if (null == cameraService.m_MyCamera)
                {
                    cameraService.m_MyCamera = new CCamera();
                    if (null == cameraService.m_MyCamera)
                    {
                        return;
                    }
                }

                //创建相机句柄
                int nRet = cameraService.m_MyCamera.CreateHandle(ref device);
                if (CErrorDefine.MV_OK != nRet)
                {
                    return;
                }

                //打开设备-->> 连接相机
                nRet = cameraService.m_MyCamera.OpenDevice();
                if (CErrorDefine.MV_OK != nRet)
                {
                    //释放相机句柄
                    cameraService.m_MyCamera.DestroyHandle();
                    cameraService.ShowErrorMsg("Device open fail!", nRet);
                    return;
                }


                // ch:探测网络最佳包大小(只对GigE相机有效) 
                if (device.nTLayerType == CSystem.MV_GIGE_DEVICE)
                {
                    //获取最合适的带宽
                    int nPacketSize = cameraService.m_MyCamera.GIGE_GetOptimalPacketSize();
                    if (nPacketSize > 0)
                    {
                        //设置最合适的带宽
                        nRet = cameraService.m_MyCamera.SetIntValue("GevSCPSPacketSize", (uint)nPacketSize);
                        if (nRet != CErrorDefine.MV_OK)
                        {
                            cameraService.ShowErrorMsg("Set Packet Size failed!", nRet);
                        }
                    }
                }

                // TODO 修改1
                // ch:设置触发模式为off 
                nRet = cameraService.m_MyCamera.SetEnumValue("TriggerMode", (uint)MV_CAM_TRIGGER_MODE.MV_TRIGGER_MODE_OFF);
                if (CErrorDefine.MV_OK != nRet)
                {
                    Console.WriteLine("Set TriggerMode failed:{0:x8}", nRet);
                }

                // TODO 修改2
                // ch:注册回调函数 ，只要有图就会进入这个方法
                cameraService.ImageCallback = new cbOutputExdelegate(ImageCallbackFunc);
                nRet = cameraService.m_MyCamera.RegisterImageCallBackEx(cameraService.ImageCallback, IntPtr.Zero);
                if (CErrorDefine.MV_OK != nRet)
                {
                    Console.WriteLine("Register image callback failed!");
                }

                // TODO 修改3
                // ch:开启抓图 || en: start grab image
                nRet = cameraService.m_MyCamera.StartGrabbing();
                if (CErrorDefine.MV_OK != nRet)
                {
                    Console.WriteLine("Start grabbing failed:{0:x8}", nRet);
                }

                //设置参数
                bnSetParam_Click(null, null);
                HandleConnectStatus(true);
                MessageBox.Show("相机连接成功！");
            }
            catch (Exception)
            {
                MessageBox.Show("相机连接失败！");
            }


        }

        /// <summary>
        /// 图像回调函数
        /// </summary>
        /// <param name="pData"></param>
        /// <param name="pFrameInfo"></param>
        /// <param name="pUser"></param>
        public void ImageCallbackFunc(IntPtr pData, ref MV_FRAME_OUT_INFO_EX pFrameInfo, IntPtr pUser)
        {
            if (null == m_hImage)
            {
                m_hImage = new HImage();
            }
            //创建一个图像 图像为byte类型 
            //图像大小 nWidth、nHeight ,图像信息 pData
            m_hImage.GenImage1("byte", pFrameInfo.nWidth, pFrameInfo.nHeight, pData);

            //黑白、彩色图像处理（彩色图像特别要注意颜色排列，4种方式）
            //黑白
            if (pFrameInfo.enPixelType == MvGvspPixelType.PixelType_Gvsp_Mono8)
            {
                m_hImage.GenImage1("byte", pFrameInfo.nWidth,
                                                         pFrameInfo.nHeight,
                                                         pData);
            }
            //彩色
            else if (pFrameInfo.enPixelType == MvGvspPixelType.PixelType_Gvsp_BayerGR8)
            {
                m_hImage = m_hImage.CfaToRgb("bayer_gr", "bilinear");
            }
            else if (pFrameInfo.enPixelType == MvGvspPixelType.PixelType_Gvsp_BayerRG8)
            {
                m_hImage = m_hImage.CfaToRgb("bayer_rg", "bilinear");
            }
            else if (pFrameInfo.enPixelType == MvGvspPixelType.PixelType_Gvsp_BayerGB8)
            {
                m_hImage = m_hImage.CfaToRgb("bayer_gb", "bilinear");
            }
            else if (pFrameInfo.enPixelType == MvGvspPixelType.PixelType_Gvsp_BayerBG8)
            {
                m_hImage = m_hImage.CfaToRgb("bayer_bg", "bilinear");
            }


            //如果是 标定 或者自动运行 有图就需要进行定位图像处理流程
            if (State.isStart_标定取图 || State.isAutoRun_自动运行)
            {
                //异步执行 不等执行完就跑到下面的代码里面
                hWindow_Final1.Invoke(new Action(() =>
                {
                    hWindow_Final1.HobjectToHimage(m_hImage);
                    button1_查找矩形_Click(null, null);
                }));
            }
            else
            {
                hWindow_Final1.HobjectToHimage(m_hImage);
            }
        }

        private void HandleConnectStatus(bool status)
        {
            //连接状态
            button_status.BackColor = status ? Color.LightGreen : Color.Gray;

        }

        /// <summary>
        /// 获取参数
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bnGetParam_Click(object sender, EventArgs e)
        {
            cameraService.GetParam(tbExposure.Text, tbGain.Text);
        }

        /// <summary>
        /// 设置参数
        /// !!!设置前需要关闭自动模式，我们要追求图像的稳定性
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bnSetParam_Click(object sender, EventArgs e)
        {
            cameraService.SetParam(tbExposure.Text, tbGain.Text);
        }



        /// <summary>
        /// 开始采集
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bnStartGrab_Click(object sender, EventArgs e)
        {
            cameraService.StartGrab(bnContinuesMode.Checked, bnTriggerSoftware.Checked, tbExposure.Text, tbGain.Text);
        }



        /// <summary>
        /// 停止采集
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void bnStopGrab_Click(object sender, EventArgs e)
        {
            try
            {
                cameraService.StopGrab();
                HandleConnectStatus(false);
                MessageBox.Show("已断开相机连接！");
            }
            catch (Exception)
            {
                MessageBox.Show("操作成功！");
            }
        }



        /// <summary>
        /// 窗口释放断开
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>

        private void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            bnStopGrab_Click(null, null);
            tcpService.FormClosing();
        }



        /// <summary>
        /// 查找相机
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void buttonFind_Click(object sender, EventArgs e)
        {
            cameraService.DeviceListAcq(cbDeviceList);
        }

        #endregion

        #region 模板匹配

        /// <summary>
        /// 创建模板
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button_创建模板_Click(object sender, EventArgs e)
        {
            CreateTemplate();
        }

        /// <summary>
        /// 创建模板方法
        /// </summary>
        /// <param name="hRegion"></param>
        private void CreateTemplate()
        {
            try
            {
                //相机采集要进行的步骤

                HOperatorSet.GetImageSize(m_hImage, out HTuple width, out HTuple height);
                //设置系统参数
                HOperatorSet.SetSystem("width", width);
                HOperatorSet.SetSystem("height", height);
                //或者直接不使用剪切，这种方式更优，不用考虑图像的大小
                //HOperatorSet.SetSystem("clip_region", "false");

                double angleStart = (double)numericUpDown_起始角度.Value;//起始角度
                double angleExtent = (double)numericUpDown_终止角度.Value - angleStart;//角度扩展（终止角度-起始角度）
                double scaleMin = (double)numericUpDown_缩小比例.Value; //缩小比例
                double scaleMax = (double)numericUpDown_放大比例.Value; //放大比例
                double minScore = (double)numericUpDown_得分.Value;//得分
                double greediness = (double)numericUpDown_贪婪度.Value;//贪婪度
                int numMatches = (int)numericUpDown_匹配个数.Value;//匹配个数

                //图像剪切
                HImage tempImage = m_hImage.ReduceDomain(region_最终区域);

                //获得轮廓
                //HXLDCont hXLDCont = tempImage.EdgesSubPix("canny", 1.0, 20, 40);

                //第二步 
                //缩放模板
                myData.hShapeModel.CreateScaledShapeModel(tempImage, //template
                                                             "auto",//NumLevels
                                                              new HTuple(angleStart).TupleRad(), //起始弧度
                                                              new HTuple(angleExtent).TupleRad(),//角度扩展（终止角度）
                                                             "auto", //AngleStep
                                                             scaleMin,//scaleMin
                                                             scaleMax,//scaleMax
                                                             "auto",//scaleStep
                                                             "auto",//Optimization
                                                             "use_polarity",//Metric
                                                             "auto",//contrast
                                                             "auto");//MinContrast


                //第三步 获得模板轮廓（默认位置在0,0,0）
                HXLDCont hXLD = myData.hShapeModel.GetShapeModelContours(1);
                // 显示模板轮廓
                hWindow_Final1.DispObj(hXLD, "red");

                //第四步 创建的时候查找
                myData.hShapeModel.FindScaledShapeModel(m_hImage,  //查找图
                    new HTuple(angleStart).TupleRad(), //起始弧度
                    new HTuple(angleExtent).TupleRad(),//角度扩展（终止角度）
                    scaleMin,  //缩小比例
                    scaleMax, //放大比例
                    minScore,  //最小得分
                    numMatches, //匹配个数
                    0.5, //重叠系数
                    "least_squares",//精细程度
                    0,//金字塔（与创建时保持一致）
                    greediness,//贪婪度 
                    out HTuple row, out HTuple column, out HTuple angle, out HTuple scale, out HTuple score);

                //可能有多个模板 循环
                for (int i = 0; i < row.Length; i++)
                {
                    //变换矩阵
                    HHomMat2D hHomMat2D = new HHomMat2D();
                    //先缩放 围绕着0,0 row col 方向缩放 scale[i] scale[i]
                    hHomMat2D = hHomMat2D.HomMat2dScale(scale[i].D, scale[i], 0, 0);

                    //应用仿射变换（模板轮廓）,不能拿模板的来装（变化后位置不在0,0）
                    HXLDCont hXLDCont1 = hHomMat2D.AffineTransContourXld(hXLD);

                    //刚性仿射变换，从（0,0,0）移动到当前 row[i], column[i], angle[i]
                    hHomMat2D = new HHomMat2D();
                    hHomMat2D.VectorAngleToRigid(0.0, 0, 0, row[i], column[i], angle[i]);

                    //对前面缩放过的轮廓仿射变换到当前位置
                    hXLDCont1 = hHomMat2D.AffineTransContourXld(hXLD);

                    //显示模板轮廓
                    hWindow_Final1.DispObj(hXLDCont1, "blue");
                    //显示中心点坐标
                    HXLDCont hXLDCont2 = new HXLDCont();
                    hXLDCont2.GenCrossContourXld(row[i].D, column[i], 20, 0.78);
                    hWindow_Final1.DispObj(hXLDCont2, "green");

                    //记录模板 A 点信息
                    myData.m_hShapeMode_row = row[i].D;
                    myData.m_hShapeMode_col = column[i].D;
                    myData.m_hShapeMode_ang = angle[i].D;
                }
                MessageBox.Show("创建成功!");
            }
            catch (Exception)
            {
                MessageBox.Show("创建失败!");
            }

        }

        /// <summary>
        /// 查找模板
        /// </summary>
        private bool FindScaledShapeModel()
        {
            try
            {
                double angleStart = (double)numericUpDown_起始角度.Value;//起始角度
                double angleExtent = (double)numericUpDown_终止角度.Value - angleStart;//角度扩展（终止角度-起始角度）
                double scaleMin = (double)numericUpDown_缩小比例.Value; //缩小比例
                double scaleMax = (double)numericUpDown_放大比例.Value; //放大比例
                double minScore = (double)numericUpDown_得分.Value;//得分
                double greediness = (double)numericUpDown_贪婪度.Value;//贪婪度
                int numMatches = (int)numericUpDown_匹配个数.Value;//匹配个数

                //第三步 获得模板轮廓（默认位置在0,0,0）
                HXLDCont hXLDCont = myData.hShapeModel.GetShapeModelContours(1);
                // 显示模板轮廓
                hWindow_Final1.DispObj(hXLDCont, "red");

                //第四步 创建的时候查找
                myData.hShapeModel.FindScaledShapeModel(m_hImage,  //查找图
                    new HTuple(angleStart).TupleRad(), //起始弧度
                    new HTuple(angleExtent).TupleRad(),//角度扩展（终止角度）
                    scaleMin,  //缩小比例
                    scaleMax, //放大比例
                    minScore,  //最小得分
                    numMatches, //匹配个数
                    0.5, //重叠系数
                    "least_squares",//精细程度
                    0,//金字塔（与创建时保持一致）
                    greediness,//贪婪度 
                    out HTuple row, out HTuple column, out HTuple angle, out HTuple scale, out HTuple score);

                //可能有多个模板 循环
                for (int i = 0; i < row.Length; i++)
                {
                    //变换矩阵
                    HHomMat2D hHomMat2D = new HHomMat2D();
                    //先缩放 围绕着0,0 row col 方向缩放 scale[i] scale[i]
                    hHomMat2D = hHomMat2D.HomMat2dScale(scale[i].D, scale[i], 0, 0);

                    //应用仿射变换（模板轮廓）,不能拿模板的来装（变化后位置不在0,0）
                    HXLDCont hXLDCont1 = hHomMat2D.AffineTransContourXld(hXLDCont);

                    //刚性仿射变换，从（0,0,0）移动到当前 row[i], column[i], angle[i]
                    hHomMat2D = new HHomMat2D();
                    hHomMat2D.VectorAngleToRigid(0.0, 0, 0, row[i], column[i], angle[i]);

                    //对前面缩放过的轮廓仿射变换到当前位置
                    hXLDCont1 = hHomMat2D.AffineTransContourXld(hXLDCont1);

                    //显示模板轮廓
                    hWindow_Final1.DispObj(hXLDCont1, "blue");
                    //显示中心点坐标
                    HXLDCont hXLDCont2 = new HXLDCont();
                    hXLDCont2.GenCrossContourXld(row[i].D, column[i], 20, 0.78);
                    hWindow_Final1.DispObj(hXLDCont2, "green");

                    //记录模板A~点信息
                    //Cur -->current  当前的
                    m_CurModel_Row = row[i].D;
                    m_CurModel_Col = column[i].D;
                    m_CurModel_Angle = angle[i].D;
                }
                return true;
            }
            catch (Exception)
            {
                MessageBox.Show("未找到模板信息！");
                return false;
            }

        }

        private void button_查找模板_Click(object sender, EventArgs e)
        {
            FindScaledShapeModel();
        }

        private void button_保存参数_Click(object sender, EventArgs e)
        {
            //保存数据，对象化流
            MyTool.Save2File(myData, saveFile);
        }

        private void button_读取参数_Click(object sender, EventArgs e)
        {
            ReadParameterData();
        }

        private void ReadParameterData()
        {
            //读取数据 流化对象
            myData = MyTool.FromByteFile<MyData>(saveFile);
            //文件不存在的时候，需要实例化
            if (myData == null)
            {
                myData = new MyData();
            }
        }


        private void button1绘制涂抹_Click(object sender, EventArgs e)
        {
            DrawImage(1);
        }

        private void button2清除涂抹_Click(object sender, EventArgs e)
        {
            DrawImage(0);
        }

        /// <summary>
        /// 涂抹 1/擦除 0
        /// </summary>
        /// <param name="type"> 涂抹 1/擦除 0 </param>
        private void DrawImage(int type)
        {
            //打开绘制模式 防止左键按下时 屏幕拖动
            hWindow_Final1.DrawModel = true;

            //1.生成跟随鼠标动的区域（涂抹区域）
            HRegion region_涂抹区域 = new HRegion();
            region_涂抹区域.GenRectangle1(0.0, 0, 10, 10);
            //用来显示轮廓和区域，颜色为green
            //hWindow_Final1.DispObj (region_涂抹区域,"green");

            //2.生成的原始区域（用来跟鼠标区域加、减）--> 最终区域
            //HRegion region_最终区域 = new HRegion();
            //region_最终区域.GenCircle(300.0, 240, 100);

            //获取涂抹区域中心坐标
            HOperatorSet.AreaCenter(region_涂抹区域, out HTuple area, out HTuple row_涂抹, out HTuple column_涂抹);

            //3.（涂抹区域）跟随鼠标动
            HTuple button = -1;
            HTuple row_鼠标 = -1;
            HTuple column_鼠标 = -1;
            HRegion region_仿射区域 = new HRegion();
            HRegion hRegion;

            //右键按下为4
            while (button != 4)
            {
                //处理当前循环的同时，也兼顾界面刷新
                Application.DoEvents();
                //更新背景图
                hWindow_Final1.HobjectToHimage(m_hImage);
                //显示最终区域，红色
                hWindow_Final1.DispObj(region_最终区域, "red");


                try
                {
                    //button：1左键按下 2滚轮按下 4右键按下
                    HOperatorSet.GetMposition(hWindow, out row_鼠标, out column_鼠标, out button);
                }
                catch (HalconException ex)
                {
                    button = -1;//拿不到鼠标位置处理
                    row_鼠标 = -1;
                    column_鼠标 = -1;
                    //MessageBox.Show(ex.Message);
                }

                //鼠标在大黑框里面
                if (row_鼠标 >= 0 && column_鼠标 >= 0)
                {
                    //仿射变换关系式，从 涂抹 —>> 鼠标
                    HHomMat2D hHomMat2D = new HHomMat2D();
                    //刚性仿射变换
                    hHomMat2D.VectorAngleToRigid(row_涂抹, column_涂抹, 0, row_鼠标, column_鼠标, 0);
                    //应用区域仿射变换
                    region_仿射区域 = hHomMat2D.AffineTransRegion(region_涂抹区域, "nearest_neighbor");

                    //显示仿射区域
                    hWindow_Final1.DispObj(region_仿射区域, "green");
                }

                //4.最终区域 与 涂抹区域 进行加/减

                //鼠标左键按下开始绘制
                if (button == 1)
                {
                    //鼠标按下生成的区域：region_仿射区域
                    //最终区域：region_最终区域
                    hRegion = type == 0 ? region_最终区域.Difference(region_仿射区域) : region_最终区域.Union2(region_仿射区域);

                    //区域是否进行初始化： new是实例化；初始化是有实际的赋值
                    if (hRegion.IsInitialized())
                    {
                        //释放区域
                        region_最终区域.Dispose();
                        region_最终区域 = hRegion;
                    }
                }

                Thread.Sleep(100);
            }
            if (button == 4)
            {
                //更新背景图
                hWindow_Final1.HobjectToHimage(m_hImage);
                //显示最终区域，红色
                hWindow_Final1.DispObj(region_最终区域, "red");
            }
            //关闭绘制模式，不关闭图像不能进行拖动、缩放
            hWindow_Final1.DrawModel = false;
        }



        private void button_绘制矩形_Click(object sender, EventArgs e)
        {
            //设置颜色
            hWindow.SetColor("red");
            //获得窗口焦点
            hWindow_Final1.Focus();
            //打开绘制模式
            hWindow_Final1.DrawModel = true;
            //绘制矩形1
            hWindow.DrawRectangle1(out double row1, out double column1, out double row2, out double column2);
            //关闭绘制模式
            hWindow_Final1.DrawModel = false;

            //创建矩形1区域
            HRegion hRegion = new HRegion();
            hRegion.GenRectangle1(row1, column1, row2, column2);
            region_最终区域 = hRegion;
            hWindow_Final1.DispObj(region_最终区域, "red");
        }

        private void button_绘制圆_Click(object sender, EventArgs e)
        {
            //设置颜色
            hWindow.SetColor("red");
            //获得窗口焦点
            hWindow_Final1.Focus();
            //打开绘制模式
            hWindow_Final1.DrawModel = true;
            //绘制矩形一
            hWindow.DrawCircle(out double row, out double column, out double radius);
            //关闭绘制模式
            hWindow_Final1.DrawModel = false;

            //创建矩形1区域
            HRegion hRegion = new HRegion();
            hRegion.GenCircle(row, column, radius);
            region_最终区域 = hRegion;
            hWindow_Final1.DispObj(region_最终区域, "red");
        }

        #endregion

        #region 测量
        private void button1矩形测量_Click(object sender, EventArgs e)
        {
            //窗口获得焦点（解决绘制直线，点第二下才出来线条）
            hWindow_Final1.Focus();
            //设置颜色
            hWindow.SetColor("red");
            //打开绘制模式（解决图像跑偏）
            hWindow_Final1.DrawModel = true;

            //1.绘制矩形
            hWindow.DrawRectangle2(out double row, out double column, out double phi, out double length1, out double length2);
            // 关闭绘制模式
            hWindow_Final1.DrawModel = false;

            MeasureRectangle(row, column, phi, length1, length2);
        }


        /// <summary>
        /// 测量矩形方法
        /// </summary>
        /// <param name="row"></param>
        /// <param name="column"></param>
        /// <param name="phi"></param>
        /// <param name="length1"></param>
        /// <param name="length2"></param>
        /// <param name="sender"></param>
        private void MeasureRectangle(double row, double column, double phi, double length1, double length2)
        {

            try
            {
                //2.创建测量模型
                //HOperatorSet.CreateMetrologyModel(out HTuple metrologyHandle);
                HMetrologyModel hMetrologyModel = new HMetrologyModel();


                //3.添加测量工具到模型中
                double measure_length1 = (double)numericUpDown_卡尺宽度.Value;
                double measure_length2 = (double)numericUpDown_卡尺高度.Value;
                double measure_distance = (double)numericUpDown_卡尺间隔.Value;
                double measure_threshold = (double)numericUpDown_灰度阈值.Value;

                //过渡方式
                string measure_transition;
                switch (comboBox1_过渡方式.SelectedText)
                {
                    case "黑到白":
                        measure_transition = "positive";
                        break;
                    case "白到黑":
                        measure_transition = "negative";
                        break;
                    default:
                        measure_transition = "all";
                        break;
                }

                //点选择
                string measure_select;
                switch (comboBox1_点选择.SelectedText)
                {
                    case "第一点":
                        measure_select = "first";
                        break;
                    case "最末点":
                        measure_select = "last";
                        break;
                    default:
                        measure_select = "all";
                        break;
                }

                int index = hMetrologyModel.AddMetrologyObjectRectangle2Measure(
                    row, column, phi, length1, length2,
                    measure_length1, measure_length2, 1,
                    measure_threshold, new HTuple(), new HTuple());

                //4.设置工具参数
                hMetrologyModel.SetMetrologyObjectParam(index, "measure_length1", measure_length1);
                hMetrologyModel.SetMetrologyObjectParam(index, "measure_length2", measure_length2);
                hMetrologyModel.SetMetrologyObjectParam(index, "measure_distance", measure_distance);
                hMetrologyModel.SetMetrologyObjectParam(index, "measure_threshold", measure_threshold);
                hMetrologyModel.SetMetrologyObjectParam(index, "measure_transition", measure_transition);
                hMetrologyModel.SetMetrologyObjectParam(index, "measure_select", measure_select);
                hMetrologyModel.SetMetrologyObjectParam(index, "min_score", 0.3);//测量工具得分

                //5.应用测量模型
                hMetrologyModel.ApplyMetrologyModel(m_hImage);

                //6.获取测量结果（小卡尺，测量工具结果）

                //小卡尺轮廓结果 （小卡尺结果 c#里面所有 xld都用 HXLDCont 类型）
                HXLDCont hXLDCont = hMetrologyModel.GetMetrologyObjectMeasures(index, "all", out HTuple row1, out HTuple column1);

                //十字架显示轮廓
                HXLDCont hxldCont1 = new HXLDCont();
                hxldCont1.GenCrossContourXld(row1, column1, 2, 0.78);

                //可视化 显示小卡尺轮廓 和小卡尺点 
                hWindow_Final1.DispObj(hXLDCont, "blue");
                hWindow_Final1.DispObj(hxldCont1, "green");

                //获取矩形测量结果
                // res 结果：中心点坐标 + 角度、半宽、半高
                HTuple res = hMetrologyModel.GetMetrologyObjectResult(index, "all", "result_type", "all_param");
                //矩形测量结果轮廓
                HXLDCont hxldCont2 = hMetrologyModel.GetMetrologyObjectResultContour(index, "all", 1.5);

                //矩形测量有结果
                if (res.Length > 4)
                {
                    HXLDCont hxldCont3 = new HXLDCont();
                    hxldCont3.GenCrossContourXld(new HTuple(res[0].D), new HTuple(res[1].D), 5, 0.78);
                    double angle = res[2].D;
                    hWindow_Final1.DispObj(hxldCont2, "red");
                    hWindow_Final1.DispObj(hxldCont3, "yellow");
                    HalconTool.disp_message(hWindow, $"角度： {angle}", "image", res[0], res[1], "red", "true");

                    if (checkBox1.Checked)
                    {
                        GraphicalData data = new GraphicalData();
                        //模板参数
                        data.Measure_select = measure_select;
                        data.Measure_transition = measure_transition;
                        data.Measure_length1 = measure_length1;
                        data.Measure_length2 = measure_length2;
                        data.Measure_threshold = measure_threshold;
                        data.Measure_distance = measure_distance;
                        //矩形参数
                        data.RectangleLength1 = length1;
                        data.RectangleLength2 = length2;
                        data.RectanglePhi = phi;
                        data.RectangleRow = row;
                        data.RectangleColumn = column;



                        // 容器[直线测量1] = lineData
                        myData.m_RectangleData["矩形测量"] = data;


                        //创建模板
                        HRegion hRegion = new HRegion();
                        hRegion.GenRectangle2(row, column, phi, length1, length2);
                        region_最终区域 = hRegion;
                        CreateTemplate();
                    }

                    //如果是标定模式
                    if (State.isStartCalib_开始标定 == true && !State.isAutoRun_自动运行 && !State.isTsetHand_手动测试)
                    {

                        //添加标定的数据显示
                        if (State.currentCB_当前标定项 == CalibrationProject.旋转中心标定)
                        {
                            CalibrationData data = new CalibrationData(Point.rotate5Index + 1,
                                res[0], res[1],
                                Point.pointX_机器人当前坐标,
                                Point.pointY_机器人当前坐标,
                                DateTime.Now);

                            myData.RobotPointData.RotateList.Add(data);
                            //列表数据更新UI
                            dataGridView2.DataSource = myData.RobotPointData.RotateList;
                            // 清空所有行的选择状态
                            int lastIndex = myData.RobotPointData.RotateList.Count - 1;
                            dataGridView2.ClearSelection();
                            dataGridView2.Rows[lastIndex].Selected = true;
                            dataGridView2.FirstDisplayedScrollingRowIndex = lastIndex;
                        }

                        if (State.currentCB_当前标定项 == CalibrationProject.九点标的)
                        {
                            CalibrationData data = new CalibrationData(Point.point9Index + 1,
                                 res[0], res[1],
                                 Point.pointX_机器人当前坐标,
                                 Point.pointY_机器人当前坐标,
                                 DateTime.Now);

                            myData.RobotPointData.CalibrationList.Add(data);
                            //列表数据更新UI
                            dataGridView1.DataSource = myData.RobotPointData.CalibrationList;
                            dataGridView1.ClearSelection();
                            int lastIndex = myData.RobotPointData.CalibrationList.Count - 1;
                            dataGridView1.Rows[lastIndex].Selected = true;
                            dataGridView1.FirstDisplayedScrollingRowIndex = lastIndex;
                        }
                    }

                    //如果是自动运行测量或者单点测量机器人取料
                    if (State.isAutoRun_自动运行 || State.isTsetHand_手动测试)
                    {
                        State.isTsetHand_手动测试 = false;
                        //计算位置信息，并把信息发给机器人
                        CalPoint2Robot(res[0].D, res[1].D, res[2].D);
                    }


                }

                State.done_图像处理完成 = true;
            }
            catch (Exception)
            {
                State.done_图像处理完成 = true;
                MessageBox.Show("测量失败！");
            }


            //测量对象

        }
        private void button1_查找矩形_Click(object sender, EventArgs e)
        {
            try
            {
                if (null != myData.m_RectangleData && null != myData.m_RectangleData["矩形测量"])
                {
                    //先查找模板
                    if (FindScaledShapeModel())
                    {
                        //再测量

                        //建立 A --> A~ 仿射变换
                        HHomMat2D hHomMat2D = new HHomMat2D();
                        hHomMat2D.VectorAngleToRigid(
                            myData.m_hShapeMode_row, //A
                            myData.m_hShapeMode_col,
                            myData.m_hShapeMode_ang,
                            m_CurModel_Row,//A~
                            m_CurModel_Col,
                            m_CurModel_Angle
                        );

                        //仿射变换后的矩形
                        GraphicalData affRectangle = AffineRectangle(hHomMat2D, myData.m_RectangleData["矩形测量"]);
                        MeasureRectangle(affRectangle.RectangleRow, affRectangle.RectangleColumn, affRectangle.RectanglePhi, affRectangle.RectangleLength1, affRectangle.RectangleLength2);
                    }
                }
            }
            catch (Exception)
            {

                MessageBox.Show("查找失败！");

            }
        }

        /// <summary>
        /// 矩形仿射
        /// </summary>
        /// <param name="hHomMat2D"></param>
        /// <param name="data"></param>
        /// <returns></returns>
        public GraphicalData AffineRectangle(HHomMat2D hHomMat2D, GraphicalData data)
        {
            //使用深度拷贝
            GraphicalData rectangle = MyTool.DeepCopyByBinary<GraphicalData>(data);

            //对矩形的终点进行仿射变换
            rectangle.RectangleRow = hHomMat2D.AffineTransPoint2d(
                data.RectangleRow,
                data.RectangleColumn,
                out rectangle.RectangleColumn);
            //矩形的角度
            rectangle.RectanglePhi = m_CurModel_Angle;

            return rectangle;
        }

        #endregion

        #region TCP连接

        int num = 0;
        /// <summary>
        /// 订阅事件
        /// </summary>
        /// <param name="temp"></param>
        /// <param name="dataBytes"></param>
        /// <exception cref="NotImplementedException"></exception>
        private void M_TcpServer_OnReceviceByte(System.Net.Sockets.Socket temp, byte[] dataBytes)
        {
            //获得当前设备的 ip  与 port
            IPAddress ip = ((IPEndPoint)temp.RemoteEndPoint).Address;
            int port = ((IPEndPoint)temp.RemoteEndPoint).Port;

            //发过来的什么东西
            //  GetString byte[]  转换成  String
            //  Replace("\r", "")  -->替换源  \r   替换成 ""
            //  .Trim();  移除字符串  前面与后面的空白符
            string msg = Encoding.Default.
                GetString(dataBytes).Replace("\r", "").Trim();

            num++;

            if (msg == "T")
            {
                //m_AutoResetEvent1.Set();//触发
                //m_AutoResetEvent2.WaitOne();//等待结果

                tcpService.ServerSendMessage($"服务端回复: {num.ToString()}");

            }
            else if (msg == "S")
            {

            }
            //年月日时分秒毫秒
            string info = DateTime.Now.ToString("yyyy_MM_dd HH:mm:ss:fff") + " " + ip.ToString() + " " + port.ToString() + " " + num.ToString();
            WriteLog_Server(ip + "_" + port + " " + info);
        }

        /// <summary>
        /// 离线通知
        /// </summary>
        /// <param name="temp"></param>
        /// <exception cref="NotImplementedException"></exception>
        private void M_TcpServer_OnOfflineClient(System.Net.Sockets.Socket temp)
        {
            //获得当前设备的 ip  与 port
            IPAddress ip = ((IPEndPoint)temp.RemoteEndPoint).Address;
            int port = ((IPEndPoint)temp.RemoteEndPoint).Port;

            WriteLog_Server(ip + "_" + port + "离线了!");
        }

        /// <summary>
        /// 写日志
        /// </summary>
        /// <param name="msg"></param>
        private void WriteLog_Server(string msg)
        {
            string str = DateTime.Now.ToString("yyyy_MM_dd HH:mm:ss fff");
            msg = $"{str}: {msg}";

            //异步线程写入日志
            textBox1.BeginInvoke(new Action(() =>
            {
                if (textBox1.TextLength > 1000)
                {
                    textBox1.Text = "";
                }
                textBox1.AppendText(msg + "\r\n");
                //翻滚到当前
                textBox1.SelectionStart = textBox1.TextLength;
                textBox1.ScrollToCaret();

            }));
        }

        /// <summary>
        /// 上线通知
        /// </summary>
        /// <param name="temp"></param>
        /// <exception cref="NotImplementedException"></exception>
        private void M_TcpServer_OnOnlineClient(System.Net.Sockets.Socket temp)
        {
            //获得当前设备的 ip  与 port
            IPAddress ip = ((IPEndPoint)temp.RemoteEndPoint).Address;
            int port = ((IPEndPoint)temp.RemoteEndPoint).Port;

            tcpService.serverIp = ip.ToString();
            tcpService.serverPort = port;

            WriteLog_Server(ip + "_" + port + "上线了!");
        }

        #endregion

        #region 定位
        /// <summary>
        /// 计算机器人最终要走的位置
        /// </summary>
        /// <param name="row">图像坐标系 row</param>
        /// <param name="col">图像坐标系 col</param>
        /// <param name="phi">图像坐标系 phi</param>
        private void CalPoint2Robot(double row, double col, double phi)
        {
            //已经标定过
            if (myData.RobotPointData.isCalibDone)
            {
                ////旋转中心转成世界坐标

                double point_机器人要走的位置X;
                double point_机器人要走的位置Y;
                double point_机器人要走的位置R;

                //旋转中心的世界坐标
                double qx1, qy1;
                qx1 = myData.RobotPointData.Template_HomMat2D.AffineTransPoint2d(myData.RobotPointData.RotateCenterX,
                                                            myData.RobotPointData.RotateCenterY, out qy1);

                //当前拍照点的世界坐标
                double qx, qy;
                //计算当前像素坐标点转换成世界坐标 -->机器人要走的位置
                qx = myData.RobotPointData.Template_HomMat2D.AffineTransPoint2d(row, col, out qy);

                //计算旋转中心到当前模板点的关系
                HHomMat2D hHomMat2D2 = new HHomMat2D();
                //是旋转中心到模板的位置  所以是 旋转中心（qx1, qy1）在前面
                hHomMat2D2.VectorAngleToRigid(qx1, qy1, 0, qx, qy, 0);

                // -176 235      492.3 692.03
                //世界坐标移动了那么多  基准点也跟着移动 就能取到料
                point_机器人要走的位置X = hHomMat2D2.AffineTransPoint2d(myData.RobotPointData.RobotStartX,
                                                                        myData.RobotPointData.RobotStartY,
                                                                        out point_机器人要走的位置Y);

                //弧度转角度
                double angle = phi * (180 / Math.PI);
                angle += myData.RobotPointData.BaseAngle;
                //计算角度 最大旋转角度为180  否则就反转 -180
                if (angle > 180)
                {
                    angle = angle - 360;
                }
                else if (angle < -180)
                {
                    angle = angle + 360;
                }

                point_机器人要走的位置R = angle;

                //给机器人发信息
                SendMsgToRobot(Command.robot_取放料 + ":" + point_机器人要走的位置X + ":" + point_机器人要走的位置Y + ":" + point_机器人要走的位置R);
                //日志
                InfoUpdata("发送数据给机器人 x:" + point_机器人要走的位置X.ToString("f3") + "_y" +
                                  point_机器人要走的位置Y.ToString("f3") + "_r" + point_机器人要走的位置R.ToString("f3"));

            }
            else
            {
                State.isAutoRun_自动运行 = false;
                MessageBox.Show("设备未进行标定");
            }
        }
        /// <summary>
        /// 更新日志显示栏
        /// </summary>
        /// <param name="msg"></param>
        private void InfoUpdata(string msg)
        {
            Task.Run(() =>
            {
                textBox1.BeginInvoke(new Action(() =>
                {
                    if (textBox1.Text.Length > 2000)
                    {
                        textBox1.Text = "";
                    }
                    textBox1.Text += msg + "\r\n";
                    textBox1.SelectionStart = textBox1.Text.Length;
                    textBox1.ScrollToCaret();//滚动条到最后
                }));
            });
        }

        /// <summary>
        /// 保存标定配置
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button3_保存配置_Click(object sender, EventArgs e)
        {
            myData.RobotPointData.BaseAngle = (double)numericUpDown1_基准角度.Value;
            myData.RobotPointData.RobotStartX = (double)numericUpDown1_机器人起点X.Value;
            myData.RobotPointData.RobotStartY = (double)numericUpDown1_机器人起点Y.Value;
            myData.RobotPointData.RobotStartR = (double)numericUpDown2__机器人起点R.Value;

            myData.RobotPointData.RobotIntervalX = (double)numericUpDown3_机器人移动间隔X.Value;
            myData.RobotPointData.RobotIntervalY = (double)numericUpDown2_机器人移动间隔Y.Value;
            myData.RobotPointData.RobotIntervalR = (double)numericUpDown1_机器人移动间隔R.Value;

            MyTool.Save2File(myData, saveFile);
        }


        /// <summary>
        /// 读取机器人标定数据
        /// </summary>
        private void ReadRobotData()
        {
            if (null != myData)
            {
                numericUpDown1_基准角度.Value = (decimal)myData.RobotPointData.BaseAngle;
                numericUpDown1_机器人起点X.Value = (decimal)myData.RobotPointData.RobotStartX;
                numericUpDown1_机器人起点Y.Value = (decimal)myData.RobotPointData.RobotStartY;
                numericUpDown2__机器人起点R.Value = (decimal)myData.RobotPointData.RobotStartR;

                numericUpDown3_机器人移动间隔X.Value = (decimal)myData.RobotPointData.RobotIntervalX;
                numericUpDown2_机器人移动间隔Y.Value = (decimal)myData.RobotPointData.RobotIntervalY;
                numericUpDown1_机器人移动间隔R.Value = (decimal)myData.RobotPointData.RobotIntervalR;

                dataGridView1.DataSource = myData.RobotPointData.CalibrationList;
                dataGridView2.DataSource = myData.RobotPointData.RotateList;
            }
        }


        /// <summary>
        /// 保存标定角度
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button2_修改角度_Click(object sender, EventArgs e)
        {
            myData.RobotPointData.BaseAngle = (double)numericUpDown1_基准角度.Value;
            MyTool.Save2File(myData, saveFile);
        }

        /// <summary>
        /// 开始标定
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button1_开始标定_Click(object sender, EventArgs e)
        {
            string result = "";
            //手动标定只重新计算仿射关系式
            if (checkBox2_手动.Checked)
            {
                result = GetMsg(CalculateCalib(myData, hWindow, checkBox2_旋转中心.Checked));
                MessageBox.Show($"手动标定{result}!");
            }
            else
            {

                double startX = (double)numericUpDown1_机器人起点X.Value;
                double startY = (double)numericUpDown1_机器人起点Y.Value;
                double startR = (double)numericUpDown2__机器人起点R.Value;
                double intervalX = (double)numericUpDown3_机器人移动间隔X.Value;
                double intervalY = (double)numericUpDown2_机器人移动间隔Y.Value;

                if (CheckValue(startX, "机器人X轴起点")
                    && CheckValue(startY, "机器人Y轴起点")
                    && CheckValue(intervalX, "机器人X轴间隔")
                    && CheckValue(intervalY, "机器人Y轴间隔"))
                {
                    myData.RobotPointData.RobotStartX = startX;
                    myData.RobotPointData.RobotStartY = startY;
                    myData.RobotPointData.RobotStartR = startR;
                    myData.RobotPointData.RobotIntervalX = intervalX;
                    myData.RobotPointData.RobotIntervalY = intervalY;
                    myData.RobotPointData.CalibrationList.Clear();
                    myData.RobotPointData.RotateList.Clear();
                    Startcalibratio(myData, dataGridView1, dataGridView2, hWindow, checkBox2_旋转中心.Checked);
                }
            }
            /*MessageBox.Show($"标定{result}!");*/
        }

        /// <summary>
        /// 开始标定
        /// </summary>
        /// <param name="myData"></param>
        /// <param name="dataGridView1"></param>
        /// <param name="dataGridView2"></param>
        /// <param name="textBox"></param>
        /// <param name="hWindow"></param>
        /// <param name="IsUse_Rotate">是否标定旋转中心</param>
        /// <returns></returns>
        public bool Startcalibratio(MyData myData, DataGridView dataGridView1, DataGridView dataGridView2, HWindow hWindow, bool IsUse_Rotate)
        {
            State.auto_标定状态 = CalibrationState.机器人回零;
            State.currentCB_当前标定项 = CalibrationProject.九点标的;
            State.isStartCalib_开始标定 = true;
            //生成九点标定坐标
            Point.point9Index = 0; //九点标定数据下标
            Point.rotate5Index = 0;
            //生成9点矩阵坐标
            GenerateNinePoints(myData.RobotPointData.RobotStartX, myData.RobotPointData.RobotStartY, myData.RobotPointData.RobotIntervalX, myData.RobotPointData.RobotIntervalY, out Tuple<double, double>[] grid);
            Task.Run(() =>
            {
                int waitCount = Environment.TickCount;
                while (State.isStartCalib_开始标定)
                {
                    Thread.Sleep(2);
                    switch (State.auto_标定状态)
                    {
                        case CalibrationState.机器人回零:
                            State.done_机器人到达标定点 = false;
                            RobotReturnToZero();
                            waitCount = Environment.TickCount;
                            InfoUpdata("机器人回零");
                            State.auto_标定状态 = CalibrationState.等待机器人回零完成;
                            break;

                        case CalibrationState.等待机器人回零完成:
                            if (State.done_机器人到达标定点)
                            {
                                State.done_机器人到达标定点 = false;
                                SendPosition2Robot(grid, Point.point9Index, myData.RobotPointData.RobotStartR);
                                waitCount = Environment.TickCount;
                                State.auto_标定状态 = CalibrationState.等待机器人到标定点;
                            }
                            else
                            {
                                if (IsTimeOut(waitCount))
                                {
                                    State.isStart_标定取图 = false;
                                    State.isStartCalib_开始标定 = false;
                                    MessageBox.Show("机器人复位超时！\r\n 请从新复位进行标定");
                                }
                            }
                            break;

                        case CalibrationState.等待机器人到标定点:
                            if (State.done_机器人到达标定点)
                            {
                                InfoUpdata("九点标定：等待机器人到标定点 " + Point.point9Index);
                                //清空图像等待下次取图
                                State.done_图像处理完成 = false;
                                State.isStart_标定取图 = true;
                                cameraService.GetImage();
                                waitCount = Environment.TickCount;
                                State.auto_标定状态 = CalibrationState.等待图像处理完成;
                            }
                            else
                            {
                                if (IsTimeOut(waitCount))
                                {
                                    State.isStart_标定取图 = false;
                                    State.isStartCalib_开始标定 = false;
                                    MessageBox.Show("机器人到达标定位超时！\r\n 请从新复位进行标定");
                                }
                            }
                            break;

                        case CalibrationState.等待图像处理完成:
                            if (State.done_图像处理完成)
                            {
                                //从0开始 到8  共9次  
                                if (Point.point9Index == 8)
                                {
                                    State.done_机器人到达标定点 = false;
                                    //九点标定完成就去做五点标定
                                    Point.rotate5Index = 0;
                                    //让机器人走到标记零点
                                    SendPosition2Robot(grid, 0, myData.RobotPointData.RobotStartR);
                                    //回到标定起点准备标五点 --> 九点的起点
                                    State.auto_标定状态 = CalibrationState.等待到达标定旋转中心位置;
                                    State.currentCB_当前标定项 = CalibrationProject.旋转中心标定;
                                }
                                else
                                {
                                    Point.point9Index++;
                                    State.done_机器人到达标定点 = false;
                                    SendPosition2Robot(grid, Point.point9Index, myData.RobotPointData.RobotStartR);
                                    //没有取够 9个点再返回去标定
                                    State.auto_标定状态 = CalibrationState.等待机器人到标定点;
                                }
                            }
                            else
                            {
                                if (IsTimeOut(waitCount))
                                {
                                    State.isStart_标定取图 = false;
                                    State.isStartCalib_开始标定 = false;
                                    MessageBox.Show("图像处理超时！\r\n 请从新复位进行标定");
                                }
                            }
                            break;

                        case CalibrationState.等待到达标定旋转中心位置:
                            if (IsUse_Rotate)
                            {
                                //机器人到达九点标定起点再把R轴（4轴）转-45度为五点标定的起点
                                if (State.done_机器人到达标定点)
                                {
                                    InfoUpdata("旋转中心标定：等待机器人到标定点 " + Point.rotate5Index);
                                    State.done_图像处理完成 = false;
                                    State.isStart_标定取图 = true;
                                    //开始标定
                                    cameraService.GetImage();
                                    State.auto_标定状态 = CalibrationState.等待五点标定图像处理完成;
                                }
                            }
                            else
                            {
                                State.auto_标定状态 = CalibrationState.标定完成;
                            }
                            break;

                        case CalibrationState.等待五点标定图像处理完成:
                            if (State.done_图像处理完成)
                            {
                                if (Point.rotate5Index == 4)
                                {
                                    State.auto_标定状态 = CalibrationState.标定完成;
                                }
                                else
                                {

                                    State.done_机器人到达标定点 = false;
                                    double angle = (myData.RobotPointData.RobotStartR - 60) + (Point.rotate5Index * myData.RobotPointData.RobotIntervalR);
                                    SendPosition2Robot(grid, 0, angle);
                                    //没有取够 5 个点再返回去标定
                                    State.auto_标定状态 = CalibrationState.等待到达标定旋转中心位置;
                                    Point.rotate5Index++;
                                }
                            }
                            break;

                        case CalibrationState.标定完成:
                            if (CalculateCalib(myData, hWindow, IsUse_Rotate))
                            {
                                MessageBox.Show("标定成功！");
                            }
                            else
                            {
                                MessageBox.Show("标定失败！");
                            }
                            InfoUpdata("标定完成 ");
                            State.isStart_标定取图 = false;
                            State.auto_标定状态 = CalibrationState.none;
                            //退出线程
                            State.isStartCalib_开始标定 = false;
                            State.currentCB_当前标定项 = CalibrationProject.none;
                            break;
                    }
                }
            });
            return false;
        }

        /// <summary>
        /// 生成一个3x3的坐标点网格，并按逆时针顺序保存在points列表中。
        /// 顺序：中心点 -> 右边中间 -> 右上角 -> 上方中间 -> 左上角 -> 左边中间 -> 左下角 -> 下方中间 -> 右下角
        /// </summary>
        /// <param name="centerX">中心点的X坐标</param>
        /// <param name="centerY">中心点的Y坐标</param>
        /// <param name="xSpacing">X轴间距</param>
        /// <param name="ySpacing">Y轴间距</param>
        /// <param name="points">用于保存坐标点的列表</param>
        public void GenerateNinePoints(double centerX, double centerY, double xSpacing, double ySpacing, out Tuple<double, double>[] grid)
        {
            // 定义3x3网格的坐标点
            grid = new Tuple<double, double>[]
            {
                Tuple.Create(centerX, centerY),                       // 中心点
                Tuple.Create(centerX + xSpacing, centerY),            // 右边中间
                Tuple.Create(centerX + xSpacing, centerY + ySpacing), // 右上角
                Tuple.Create(centerX + xSpacing, centerY),            // 上方中间
                Tuple.Create(centerX - xSpacing, centerY + ySpacing), // 左上角
                Tuple.Create(centerX - xSpacing, centerY),            // 左边中间
                Tuple.Create(centerX - xSpacing, centerY - ySpacing), // 左下角
                Tuple.Create(centerX, centerY - ySpacing),             // 下方中间
                Tuple.Create(centerX + xSpacing, centerY - ySpacing), // 右下角
   
            };

        }

        /// <summary>
        /// 机器人回零
        /// </summary>
        public void RobotReturnToZero()
        {
            SendMsgToRobot(Command.robot_回原点 + ":" + "0000.000" + ":" + "0000.000" + ":" + "0000.000");
        }

        /// <summary>
        /// 计算标定结果
        /// </summary>
        /// <param name="myData"></param>
        /// <param name="hWindow"></param>
        /// <param name="IsUse_Rotate">是否标定旋转中心</param>
        /// <returns></returns>
        public bool CalculateCalib(MyData myData, HWindow hWindow, bool IsUse_Rotate)
        {
            bool res = CalNinePointCalibration(myData);
            myData.RobotPointData.IsUse_Rotate = res;
            if (!res)
            {
                return false;
            }
            if (IsUse_Rotate)
            {
                res = CalRotationCenter(myData, hWindow);
                myData.RobotPointData.isCalibDone = res;
            }
            return res;
        }

        /// <summary>
        /// 计算旋转中心
        /// </summary>
        /// <param name="myData"></param>
        /// <returns></returns>
        public bool CalRotationCenter(MyData myData, HWindow hWindow)
        {
            try
            {
                if (null == myData.RobotPointData.RotateList || myData.RobotPointData.RotateList.Count < 2)
                {
                    MessageBox.Show("计算旋转中心数据过少！");
                    return false;
                }
                HTuple RPx = new HTuple();
                HTuple RPy = new HTuple();
                HTuple RQx = new HTuple();
                HTuple RQy = new HTuple();

                foreach (var item in myData.RobotPointData.RotateList)
                {
                    RPx.Append(item.PicX);//图像坐标
                    RPy.Append(item.PicY);

                    RQx.Append(item.RobX);//机器坐标
                    RQy.Append(item.RobY);
                }



                HTuple row, col, radius, startphi, endphi, pointorder;
                HObject hXLDCont = new HObject();
                //根据点创建任意轮廓
                HOperatorSet.GenContourPolygonXld(out hXLDCont, RPx, RPy);
                HOperatorSet.FitCircleContourXld(hXLDCont, "algebraic", -1, 0, 0, 3, 2, out row, out col, out radius,
                    out startphi, out endphi, out pointorder);
                HOperatorSet.GenCircleContourXld(out hXLDCont, row, col, radius, 0, 6.28, "negative", 1);

                //显示轮廓
                HXLDCont hxld1 = new HXLDCont();
                hxld1.GenCrossContourXld(row, col, 160, 0.78);
                hWindow.DispObj(hxld1);

                //保存旋转中心信息
                myData.RobotPointData.RotateCenterX = row;
                myData.RobotPointData.RotateCenterY = col;
                return true;
            }
            catch (Exception ex)
            {
                MessageBox.Show("旋转中心标定失败:" + ex.ToString());
                return false;
            }

        }

        /// <summary>
        /// 九点标定仿射变换关系计算
        /// </summary>
        /// <param name="myData"></param>
        /// <returns></returns>
        public bool CalNinePointCalibration(MyData myData)
        {
            try
            {
                if (null == myData.RobotPointData.CalibrationList || myData.RobotPointData.CalibrationList.Count < 9)
                {
                    MessageBox.Show("计算9点标定数据过少！");
                    return false;
                }

                HTuple Px = new HTuple();
                HTuple Py = new HTuple();
                HTuple Qx = new HTuple();
                HTuple Qy = new HTuple();

                foreach (var item in myData.RobotPointData.CalibrationList)
                {
                    Px.Append(item.PicX);//图像坐标
                    Py.Append(item.PicY);

                    Qx.Append(item.RobX);//机器坐标
                    Qy.Append(item.RobY);
                }
                myData.RobotPointData.Template_HomMat2D.VectorToHomMat2d(Px, Py, Qx, Qy);
                return true;
            }
            catch (Exception ex)
            {
                MessageBox.Show("九点标定失败:" + ex.ToString());
                return false;
            }
        }

        /// <summary>
        /// 机器人走标定点位
        /// </summary>
        /// <param name="CalibrationList"></param>
        /// <param name="index"></param>
        /// <param name="robR"></param>
        private void SendPosition2Robot(Tuple<double, double>[] grid, int index, double robR)
        {
            SendMsgToRobot(Command.robot_走点位 + ":" + grid[index].Item1 + ":" + grid[index].Item2 + ":" + robR);
            Point.pointX_机器人当前坐标 = grid[index].Item1;
            Point.pointY_机器人当前坐标 = grid[index].Item2;
        }

        /// <summary>
        /// 给机器人发消息
        /// </summary>
        /// <param name="msg"></param>
        public void SendMsgToRobot(string msg)
        {
            /*if (tcpService.GetClientState())
            {*/
            tcpService.ClientSend(msg);


            /* }
             else
             {
                 MessageBox.Show("机器人未连接！");
             }*/
        }

        /// <summary>
        /// 超时判断
        /// </summary>
        /// <param name="startTime">起始时间</param>
        /// <param name="delayTime">延时时间</param>
        /// <returns></returns>
        public bool IsTimeOut(int startTime)
        {
            if (Environment.TickCount - startTime > outTime)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// 机器人复位
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button1_机器人复位_Click(object sender, EventArgs e)
        {
            //发送信息让机器人复位
            RobotReturnToZero();
        }

        /// <summary>
        /// 连接机器人
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button1_连接机器人_Click(object sender, EventArgs e)
        {
            tcpService.OnlineServe();
            tcpService.DataReceived += TcpService_DataReceived;
        }

        /// <summary>
        /// 接受机器人发送的坐标处理
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void TcpService_DataReceived(object sender, string e)
        {
            UpdateNumericUpDown(e);
        }


        /// <summary>
        /// 更新机器人当前坐标
        /// </summary>
        /// <param name="data"></param>
        private void UpdateNumericUpDown(string data)
        {
           
            if (this.InvokeRequired)
            {
                this.Invoke(new Action<string>(UpdateNumericUpDown), data);
            }
            else
            {
                char[] vs1 = { ':' };
                string[] vs2 = data.Split(vs1, StringSplitOptions.RemoveEmptyEntries);
                if (vs2.Length > 0)
                {
                    //获取机器人发送的点位  格式 X:  600.000 Y:    0.000 Z:    0.000 U:    0.000 V:    0.000 W:    0.000 /R /0
                    if ("X" == vs2[0])
                    {
                        string x = vs2[1].ToString().Trim();
                        x = x.Substring(0, x.Length - 1);
                        string y = vs2[2].ToString().Trim();
                        y = y.Substring(0, y.Length - 1);
                        string u = vs2[4].ToString().Trim();
                        u = u.Substring(0, u.Length - 1);
                        //更新界面
                        numericUpDown1_机器人起点X.Value = (decimal)Convert.ToDouble(x);
                        numericUpDown1_机器人起点Y.Value = (decimal)Convert.ToDouble(y);
                        numericUpDown2__机器人起点R.Value = (decimal)Convert.ToDouble(u);
                    }

                }
            }
        }

        /// <summary>
        /// 发送信息给机器人获取它当前坐标
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button2_机器人标定起点_Click(object sender, EventArgs e)
        {

            //发送消息给机器人获得当前机器人坐标
            tcpService.ClientSend($"{Command.robot_当前点}:0000.000:0000.000:0000.000");  
        }

        private bool CheckValue(double value, string buttonMsg)
        {
            if (value == 0)
            {
                MessageBox.Show($"{buttonMsg}值不能为空！");
                return false;
            }
            return true;
        }

        private string GetMsg(bool value)
        {
            return value ? "成功" : "失败";
        }



        #endregion

        #region 取料测试
        private void button2_采集图像_Click(object sender, EventArgs e)
        {
            bnStartGrab_Click(null, null);
        }

        private void button_取料测试_Click(object sender, EventArgs e)
        {
            if (myData.RobotPointData.isCalibDone)
            {
                State.isTsetHand_手动测试 = true;
                button1_查找矩形_Click(null, null);
            }
            else
            {
                MessageBox.Show("设备未进行标定", "提示", MessageBoxButtons.OK, MessageBoxIcon.Warning);
            }
        }

        private void button_自动运行_Click(object sender, EventArgs e)
        {
            if (!_auto_function_thread)
            {
                _auto_function_thread = true;
                ThreadPool.QueueUserWorkItem(new WaitCallback(Task_AutoRun));
            }
            if (button_自动运行.Text == "自动运行")
            {
                State.auto_取放料 = Auto_取放料.等待机器人回零完成;
                State.isAutoRun_自动运行 = true;
                button_自动运行.Text = "停止运行";
            }
            else
            {
                State.isAutoRun_自动运行 = false;
                button_自动运行.Text = "自动运行";
            }
        }

        private void button_启动服务_Click(object sender, EventArgs e)
        {
            try
            {
                tcpService.StartSever();
                //绑定上线通知
                tcpService.m_TcpServer.OnOnlineClient += M_TcpServer_OnOnlineClient;
                // 离线通知
                tcpService.m_TcpServer.OnOfflineClient += M_TcpServer_OnOfflineClient;
                // 订阅事件
                tcpService.m_TcpServer.OnReceviceByte += M_TcpServer_OnReceviceByte;
                MessageBox.Show("连接成功！", "提示", MessageBoxButtons.OK);
            }
            catch (Exception)
            {
                MessageBox.Show("连接失败！", "错误提示", MessageBoxButtons.OK);

            }

        }

        private void button_关闭服务_Click(object sender, EventArgs e)
        {
            tcpService.CloseServer();
            //解除绑定上线通知
            tcpService.m_TcpServer.OnOnlineClient -= M_TcpServer_OnOnlineClient;
            // 离线通知
            tcpService.m_TcpServer.OnOfflineClient -= M_TcpServer_OnOfflineClient;
            // 订阅事件
            tcpService.m_TcpServer.OnReceviceByte -= M_TcpServer_OnReceviceByte;
        }


        private void button_验证标定_Click(object sender, EventArgs e)
        {
            Task.Run(() =>
            {
                List<double> calX = new List<double>();
                List<double> calY = new List<double>();
                int i = 0;
                foreach (CalibrationData item in myData.RobotPointData.CalibrationList)
                {
                    i++;
                    //图像坐标
                    double pixRow = item.PicX;
                    double pixCol = item.PicY;
                    //机械坐标
                    double worldX = item.RobX;
                    double worldY = item.RobY;

                    //图像坐标的世界坐标
                    double qx1, qy1;
                    qx1 = myData.RobotPointData.Template_HomMat2D.AffineTransPoint2d(pixRow, pixCol, out qy1);

                    InfoUpdata(i.ToString() + "X:" + (worldX - qx1).ToString("f3") + " _Y:" + (worldY - qy1).ToString("f3"));
                    Thread.Sleep(50);

                    calX.Add(Math.Round(Math.Abs(worldX - qx1), 2));
                    calY.Add(Math.Round(Math.Abs(worldY - qy1), 2));
                }

                InfoUpdata("均值 X:" + calX.Average().ToString("f3") + " _Y:" + calY.Average().ToString("f3"));
            });
        }

        private void Task_AutoRun(object obj)
        {
            while (true)
            {
                Thread.Sleep(2);
                int waitCount = Environment.TickCount;

                if (State.isAutoRun_自动运行)
                {
                    Thread.Sleep(2);
                    switch (State.auto_取放料)
                    {
                        case Auto_取放料.机器人回零:
                            State.done_机器人到达标定点 = false;
                            RobotReturnToZero();
                            waitCount = Environment.TickCount;
                            InfoUpdata("机器人回零");
                            State.auto_取放料 = Auto_取放料.等待机器人回零完成;
                            break;

                        case Auto_取放料.等待机器人回零完成:
                            if (State.done_机器人到达标定点)
                            {
                                waitCount = Environment.TickCount;
                                State.auto_取放料 = Auto_取放料.触发拍照;
                            }
                            else
                            {
                                if (IsTimeOut(waitCount))
                                {
                                    State.isAutoRun_自动运行 = false;
                                    MessageBox.Show("机器人复位超时！\r\n 请从新复位运行");
                                }
                            }
                            break;

                        case Auto_取放料.触发拍照:
                            State.done_图像处理完成 = false;
                            State.done_机器人到达标定点 = false;
                            button1_查找矩形_Click(null, null);
                            waitCount = Environment.TickCount;
                            State.auto_取放料 = Auto_取放料.等待取料信号;
                            break;

                        case Auto_取放料.等待取料信号:
                            if (State.done_图像处理完成)
                            {
                                waitCount = Environment.TickCount;
                                State.auto_取放料 = Auto_取放料.等待机器人放料完成;
                                InfoUpdata("机器人取料");
                            }
                            else
                            {
                                if (IsTimeOut(waitCount))
                                {
                                    State.isAutoRun_自动运行 = false;
                                    MessageBox.Show("图像处理超时！\r\n 请从新复位运行");
                                }
                            }
                            break;
                        case Auto_取放料.等待机器人放料完成:
                            if (State.done_机器人到达标定点)
                            {
                                waitCount = Environment.TickCount;
                                State.auto_取放料 = Auto_取放料.完成;
                            }
                            else
                            {
                                if (IsTimeOut(waitCount))
                                {
                                    State.isAutoRun_自动运行 = false;
                                    MessageBox.Show("机器人取放料超时！\r\n 请从新复位运行");
                                }
                            }
                            break;

                        case Auto_取放料.完成:
                            waitCount = Environment.TickCount;
                            State.auto_取放料 = Auto_取放料.触发拍照;
                            break;
                    }
                }

            }
        }



        #endregion


    }
}
